
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/mcpwm.h"
#include "tmDefIO.h"

void TmPwmInit(uint32_t freq)
{
    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM0A, TM_AXIS_X_STEP); // To drive a RC servo, one MCPWM generator is enough
    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM1A, TM_AXIS_Y_STEP);
    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM2A, TM_AXIS_Z_STEP);
    // mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, 9);

    mcpwm_config_t pwm_config = {
        .frequency = freq, // frequency = 50Hz, i.e. for every servo motor time period should be 20ms
        .cmpr_a = 0,     // duty cycle of PWMxA = 0
        .counter_mode = MCPWM_UP_COUNTER,
        .duty_mode = MCPWM_DUTY_MODE_0,
    };
    mcpwm_init(MCPWM_UNIT_1, MCPWM_TIMER_0, &pwm_config);
    mcpwm_init(MCPWM_UNIT_1, MCPWM_TIMER_1, &pwm_config);
    mcpwm_init(MCPWM_UNIT_1, MCPWM_TIMER_2, &pwm_config);

    mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_OPR_A, 50);
    mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_1, MCPWM_OPR_A, 50);
    mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_2, MCPWM_OPR_A, 50);
    TmPwmStopAll();
}

void TmPwmStartAll()
{
    mcpwm_start(MCPWM_UNIT_1, MCPWM_TIMER_0);
    mcpwm_start(MCPWM_UNIT_1, MCPWM_TIMER_1);
    mcpwm_start(MCPWM_UNIT_1, MCPWM_TIMER_2);
}

void TmPwmStopAll()
{
    mcpwm_stop(MCPWM_UNIT_1, MCPWM_TIMER_0);
    mcpwm_stop(MCPWM_UNIT_1, MCPWM_TIMER_1);
    mcpwm_stop(MCPWM_UNIT_1, MCPWM_TIMER_2);
}

// void tmPwmChangePerior(int axisIndex, uint32_t perior)
// {
//     mcpwm_set_frequency(MCPWM_UNIT_1, MCPWM_TIMER_0, perior);
//     mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_OPR_A, 50);
// }

void TmPwmChangeFreqX(uint32_t perior)
{
    mcpwm_set_frequency(MCPWM_UNIT_1, MCPWM_TIMER_0, perior);
    mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_OPR_A, 50);
}

void TmPwmChangeFreqY(uint32_t perior)
{
    mcpwm_set_frequency(MCPWM_UNIT_1, MCPWM_TIMER_1, perior);
    mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_1, MCPWM_OPR_A, 50);
}

void TmPwmChangeFreqZ(uint32_t perior)
{
    mcpwm_set_frequency(MCPWM_UNIT_1, MCPWM_TIMER_2, perior);
    mcpwm_set_duty(MCPWM_UNIT_1, MCPWM_TIMER_2, MCPWM_OPR_A, 50);
}